#include <math.h>
#include <time.h>
#include <fstream>

#include <chrono>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/line_descriptor/descriptor.hpp>
//#include <eigen3/Eigen/Core>

#include "PairwiseLineMatching.hh"
#include "LineDescriptor.hh"
#include "EDLineDetector.hh"

using namespace std;
using namespace cv;
using namespace cv::line_descriptor;
//using namespace Eigen;


int DescriptorDistance(const Mat &a, const Mat &b)
{
    const int *pa = a.ptr<int32_t>();
    const int *pb = b.ptr<int32_t>();

    int dist=0;

    for(int i=0; i<8; i++, pa++, pb++)
    {
        unsigned  int v = *pa ^ *pb;
        v = v - ((v >> 1) & 0x55555555);
        v = (v & 0x33333333) + ((v >> 2) & 0x33333333);
        dist += (((v + (v >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24;
    }

    return dist;
}

struct compare_descriptor_by_NN_dist
{
    inline bool operator()(const std::vector<cv::DMatch>& a, const std::vector<cv::DMatch>& b){
        return ( a[0].distance < b[0].distance);
    }
};

struct conpare_descriptor_by_NN12_dist
{
    inline bool operator()(const std::vector<cv::DMatch>& a, const std::vector<cv::DMatch>& b){
        return ((a[1].distance - a[0].distance) > (b[1].distance - b[0].distance));
    }
};
struct sort_descriptor_by_queryIdx
{
    inline bool operator()(const std::vector<cv::DMatch>& a, const std::vector<cv::DMatch>& b){
        return ( a[0].queryIdx < b[0].queryIdx );
    }
};

void lineDescriptorMAD(vector<vector<DMatch>> line_matches, double &nn_mad, double &nn12_mad)  {
    vector<vector<DMatch>> matches_nn, matches_12;
    matches_nn = line_matches;
    matches_12 = line_matches;

    // estimate the NN's distance standard deviation
    double nn_dist_median;
    sort(matches_nn.begin(), matches_nn.end(), compare_descriptor_by_NN_dist());
    nn_dist_median = matches_nn[int(matches_nn.size() / 2)][0].distance;

    for (unsigned int i = 0; i < matches_nn.size(); i++)
        matches_nn[i][0].distance = fabsf(matches_nn[i][0].distance - nn_dist_median);
    sort(matches_nn.begin(), matches_nn.end(), compare_descriptor_by_NN_dist());
    nn_mad = 1.4826 * matches_nn[int(matches_nn.size() / 2)][0].distance;

    // estimate the NN's 12 distance standard deviation
    double nn12_dist_median;
    sort(matches_12.begin(), matches_12.end(), conpare_descriptor_by_NN12_dist());
    nn12_dist_median =
            matches_12[int(matches_12.size() / 2)][1].distance - matches_12[int(matches_12.size() / 2)][0].distance;
    for (unsigned int j = 0; j < matches_12.size(); j++)
        matches_12[j][0].distance = fabsf(matches_12[j][1].distance - matches_12[j][0].distance - nn12_dist_median);
    sort(matches_12.begin(), matches_12.end(), compare_descriptor_by_NN_dist());
    nn12_mad = 1.4826 * matches_12[int(matches_12.size() / 2)][0].distance;
}

int SearchByDescriptor(vector<KeyLine>& linesInLeft, vector<KeyLine>& linesInRight,
                       Mat& left_ldesc, Mat& right_ldesc, vector<unsigned int>& matchresult)
{

    int nmatches = 0;
    BFMatcher* bfm = new BFMatcher(NORM_HAMMING, false);
    vector<vector<DMatch>> lmatches;
    bfm->knnMatch(left_ldesc, right_ldesc, lmatches, 2);

    double nn_dist_th, nn12_dist_th;
    const float minRatio=1.0f/1.5f;
    lineDescriptorMAD(lmatches, nn_dist_th, nn12_dist_th);
    nn12_dist_th = nn12_dist_th*0.5;
    sort(lmatches.begin(), lmatches.end(), sort_descriptor_by_queryIdx());
    for(int i=0; i<lmatches.size(); i++)
    {
        int qdx = lmatches[i][0].queryIdx;
        int tdx = lmatches[i][0].trainIdx;
        double dist_12 = lmatches[i][0].distance/lmatches[i][1].distance;
        if(dist_12<minRatio)
        {
            matchresult.push_back(qdx);
            matchresult.push_back(tdx);
            nmatches++;
        }
    }
    return nmatches;
}


void LSDExtractLineSegment(cv::Mat& image, vector<KeyLine>& keylines, float scale, int numOctaves, Mat &ldesc){
    //mpLineSegment->ExtractLineSegment(im, mvKeylinesUn, mLdesc, mvKeyLineFunctions);float scale = 1.2, int numOctaves = 1
    double t = (double)cv::getTickCount();

    Ptr<BinaryDescriptor> lbd = BinaryDescriptor::createBinaryDescriptor();//line_descriptor::BinaryDescriptor类实现了检测线和计算其二进制描述符的功能
    Ptr<line_descriptor::LSDDetector> lsd = line_descriptor::LSDDetector::createLSDDetector();
    lsd->detect(image, keylines, scale, numOctaves);  //得到 startpoint  endpoint
    t = ((double)cv::getTickCount() - t)/cv::getTickFrequency();
    std::cout<<"LSD time line extraction: "<<t<<"s"<<std::endl;
    std::cout<<"lsd extract keyline size:  "<<keylines.size()<<endl;

//    unsigned int lsdNFeatures = 40;
//
//    // filter lines
//    // kiryueshan 这里一定会保证有40条以上吗  低于的话不用重排吗
//    if (keylines.size() > lsdNFeatures) {
//        sort(keylines.begin(), keylines.end(), sort_lines_by_response());   //
//        keylines.resize(lsdNFeatures);
//        for (unsigned int i = 0; i < lsdNFeatures; i++)
//            keylines[i].class_id = i;
//    }
//
    lbd->compute(image, keylines, ldesc);  //计算描述线特征的二进制描述子
//
//    for (vector<KeyLine>::iterator it = keylines.begin(); it != keylines.end(); ++it) {
//        Vector3d sp_l;
//        sp_l << it->startPointX, it->startPointY, 1.0;
//        Vector3d ep_l;
//        ep_l << it->endPointX, it->endPointY, 1.0;
//        Vector3d lineF;
//        lineF << sp_l.cross(ep_l);
//        // (x1,y1,1)X(x2,y2,1) = (y1-y2,-x1+x2,-x2y1+x1y2)
//        lineF = lineF / sqrt(lineF(0) * lineF(0) + lineF(1) * lineF(1) + lineF(2) * lineF(2));
//        // 归一化直线
//        keylineFunctions.push_back(lineF);
//    }
}
struct sort_lines_by_response
{
    inline bool operator()(const cv::line_descriptor::KeyLine& a, const cv::line_descriptor::KeyLine& b){
        return ( a.response > b.response );
    }
};
void LSDExtractLineSegmentImprove(cv::Mat& image, vector<KeyLine>& keylines, float scale, int numOctaves, Mat &ldesc){
    //mpLineSegment->ExtractLineSegment(im, mvKeylinesUn, mLdesc, mvKeyLineFunctions);float scale = 1.2, int numOctaves = 1
//#define debugmesg

    float sTH = 30;   //线段长度阈值
    float angTH = 0.087266389*2;  //主方向阈值5degree
    float dTH = 10;  //点线距离阈值
    float lTH = 200;  //端点距离阈值
    float desTH = 80;   //描述子距离


    vector<KeyLine> lines;
//    Ptr<BinaryDescriptor> lbd = BinaryDescriptor::createBinaryDescriptor();//line_descriptor::BinaryDescriptor类实现了检测线和计算其二进制描述符的功能
//    Ptr<line_descriptor::LSDDetector> lsd = line_descriptor::LSDDetector::createLSDDetector();
//    lsd->detect(image, lines, scale, numOctaves);  //得到 startpoint  endpoint
//    lbd->compute(image, lines, ldesc);  //计算描述线特征的二进制描述子


    LineDescriptor ld;
    cv::Mat img = image.clone();
    ScaleLines keyLines;
    double t = (double)cv::getTickCount();
    if(!ld.OctaveKeyLines(img,keyLines)){
        cout<<"OctaveKeyLines failed"<<endl;
        return;
    }
    t = ((double)cv::getTickCount() - t)/cv::getTickFrequency();
    std::cout<<"edline binary time line extraction: "<<t<<"s"<<std::endl;
    std::cout<<"edline binary extract keyline size:  "<<keyLines.size()<<endl;

    int class_counter = -1;
    for(int i=0;i<keyLines.size();i++){
//        for(int j=0;j<keyLines[i].size();j++){
        KeyLine kl;

        kl.startPointX = keyLines[i][0].startPointX;
        kl.startPointY = keyLines[i][0].startPointY;
        kl.endPointX = keyLines[i][0].endPointX;
        kl.endPointY = keyLines[i][0].endPointY;
        kl.sPointInOctaveX = keyLines[i][0].sPointInOctaveX;
        kl.sPointInOctaveY = keyLines[i][0].sPointInOctaveY;
        kl.ePointInOctaveX = keyLines[i][0].ePointInOctaveX;
        kl.ePointInOctaveY = keyLines[i][0].ePointInOctaveY;

        kl.lineLength = keyLines[i][0].lineLength;
//            kl.lineLength = (float) sqrt( pow( kl.sPointInOctaveX - kl.ePointInOctaveX, 2 )
//                                          + pow( kl.sPointInOctaveY - kl.ePointInOctaveY, 2 ) );
        kl.numOfPixels = keyLines[i][0].numOfPixels;
        kl.angle = keyLines[i][0].direction;
        kl.class_id = ++class_counter;
        kl.octave = 0;
        kl.size = ( kl.endPointX - kl.startPointX ) * ( kl.endPointY - kl.startPointY );
        kl.response = kl.lineLength;
        kl.pt = Point2f( ( kl.endPointX + kl.startPointX ) / 2, ( kl.endPointY + kl.startPointY ) / 2 );
        lines.push_back(kl);
//        }
    }
    std::cout<<"binary edline extract keyline size:  "<<lines.size()<<endl;

    t = (double)cv::getTickCount();
    Ptr<BinaryDescriptor> lbd = BinaryDescriptor::createBinaryDescriptor();//line_descriptor::BinaryDescriptor类实现了检测线和计算其二进制描述符的功能
//    unsigned int lsdNFeatures = 400;
//    if (lines.size() > lsdNFeatures) {
//        sort(lines.begin(), lines.end(), sort_lines_by_response());   //
//        lines.resize(lsdNFeatures);
//        for (unsigned int i = 0; i < lsdNFeatures; i++)
//            lines[i].class_id = i;
//    }
    lbd->compute(image, lines, ldesc);  //计算描述线特征的二进制描述子

    t = ((double)cv::getTickCount() - t)/cv::getTickFrequency();
    std::cout<<"time binary lbd descriptor extraction: "<<t<<"s"<<std::endl;

#ifdef debugmesg
    cout<<"lsd detect lines:   "<<lines.size()<<endl;
#endif


    bool cflag = true;
    vector<KeyLine> linetmp;
    while(cflag){
        cflag = false;
        vector<bool> mergeflag(lines.size(),false);
        linetmp.clear();
        Mat ldesctmp;
        for(int i=0;i<lines.size();i++){
            if(mergeflag[i]==true) continue;
            float angi = lines[i].angle;
            float A_factor = lines[i].endPointY-lines[i].startPointY;
            float B_factor = -(lines[i].endPointX-lines[i].startPointX);
            float C_factor = -(lines[i].endPointY*lines[i].startPointX
                               -lines[i].startPointY*lines[i].endPointX);
            float leni = sqrt((A_factor*A_factor)+(B_factor*B_factor));
            for(int j=i+1;j<lines.size();j++){
                if(mergeflag[j]==true) continue;
                int d_des = DescriptorDistance(ldesc.row(i),ldesc.row(j));
#ifdef debugmesg
                cout<<"desTH:  "<<desTH<<"   descriptor:  "<<d_des<<endl;
#endif

                //if(d_des>desTH) continue;
                float d_angle;
                float angj = lines[j].angle;
                if(angj*angi<0){
                    if(angj<0) angj+=M_PI;
                    else angj-=M_PI;
                }
                d_angle = abs(angj-angi);
#ifdef debugmesg
                cout<<"angTH:   "<<angTH<<"   angle:  "<<angi<<"  "<<angj<<"  "<<d_angle<<endl;
#endif
//                if(angj*angi>0){
//                    d_angle = abs(angj-angi);
//                    cout<<"angTH:   "<<angTH<<"   angle:  "<<angi<<"  "<<angj<<"  "<<d_angle<<endl;
//                    //if(d_angle>angTH) continue;
//                }
//                else{
//                    d_angle = MIN(abs(angj-angi),
//                                        MIN(abs(angj-angi+2*M_PI),abs(angj-angi-2*M_PI)));
//                    cout<<"angTH:   "<<angTH<<"   angle:  "<<angi<<"  "<<angj<<"  "<<d_angle<<endl;
//                    //if(d_angle>angTH) continue;
//                }

                float midpointX = (lines[j].startPointX+lines[j].endPointX)/2;
                float midpointY = (lines[j].startPointY+lines[j].endPointY)/2;
                float d_dis = abs(A_factor*midpointX + B_factor*midpointY + C_factor)/leni;
#ifdef debugmesg
                cout<<"dTH:  "<<dTH<<"   midpoint distance d_dis:   "<<d_dis<<endl;
#endif

                //if(d_dis>dTH) continue;


                float l_dis = MIN(MIN((lines[i].startPointY-lines[j].startPointY)*(lines[i].startPointY-lines[j].startPointY)
                                      +(lines[i].startPointX-lines[j].startPointX)*(lines[i].startPointX-lines[j].startPointX),
                                      (lines[i].startPointY-lines[j].endPointY)*(lines[i].startPointY-lines[j].endPointY)
                                      +(lines[i].startPointX-lines[j].endPointX)*(lines[i].startPointX-lines[j].endPointX)),
                                  MIN((lines[i].endPointY-lines[j].startPointY)*(lines[i].endPointY-lines[j].startPointY)
                                      +(lines[i].endPointX-lines[j].startPointX)*(lines[i].endPointX-lines[j].startPointX),
                                      (lines[i].endPointY-lines[j].endPointY)*(lines[i].endPointY-lines[j].endPointY)
                                      +(lines[i].endPointX-lines[j].endPointX)*(lines[i].endPointX-lines[j].endPointX)));
#ifdef debugmesg
                cout<<"lTH:   "<<lTH<<"   line endpoint distance    "<<l_dis<<endl;
#endif
                //if(l_dis>lTH) continue;

                if(d_des>desTH||d_angle>angTH||d_dis>dTH||l_dis>lTH) continue;

                LineIterator li(image,Point2f(lines[i].startPointX,lines[i].startPointY),
                                Point2f(lines[i].endPointX,lines[i].endPointY));
                LineIterator lj(image,Point2f(lines[j].startPointX,lines[j].startPointY),
                                Point2f(lines[j].endPointX,lines[j].endPointY));

                vector<Point> pts;
                for(int pnum=0;pnum<li.count;pnum++,li++){
                    pts.push_back(li.pos());
                }
                for(int pnum=0;pnum<lj.count;pnum++,lj++){
                    pts.push_back(lj.pos());
                }
                Vec4f fit_line;
                float minx,maxx,miny,maxy;
                fitLine(pts,fit_line,CV_DIST_HUBER,0,0.01,0.01);
                if(abs(fit_line[0])>=abs(fit_line[1])){
                    float k = fit_line[1]/fit_line[0];
                    float b = fit_line[3] - k*fit_line[2];
                    // y = kx + b;
                    minx = MIN(MIN(lines[i].startPointX,lines[i].endPointX),
                                     MIN(lines[j].startPointX,lines[j].endPointX));
                    maxx = MAX(MAX(lines[i].startPointX,lines[i].endPointX),
                                     MAX(lines[j].startPointX,lines[j].endPointX));
                    miny = k*minx + b;
                    maxy = k*maxx + b;
                }
                else{
                    float kinv = fit_line[0]/fit_line[1];
                    float binv = kinv*fit_line[3] - fit_line[2];
                    // 1/k * y = x + binv;
                    miny = MIN(MIN(lines[i].startPointY,lines[i].endPointY),
                                     MIN(lines[j].startPointY,lines[j].endPointY));
                    maxy = MAX(MAX(lines[i].startPointY,lines[i].endPointY),
                                     MAX(lines[j].startPointY,lines[j].endPointY));
                    minx = kinv*miny - binv;
                    maxx = kinv*maxy - binv;
                }
                Point2f p1(minx,miny);
                Point2f p2(maxx,maxy);
                Point2f sp,ep;
                float ang1 = atan2(maxy-miny,maxx-minx);
                float ang2 = atan2(miny-maxy,minx-maxx);
                float ang = MIN(abs(ang1-angi),
                                MIN(abs(ang1-angi+2*M_PI),abs(ang1-angi-2*M_PI)));
                if(ang<M_PI/4){
                    ang = ang1;
                    sp = p1;
                    ep = p2;
                }
                else{
                    ang = ang2;
                    sp = p2;
                    ep = p1;
                }
                KeyLine kl;
                kl.angle = ang;
                kl.ePointInOctaveX = kl.endPointX = ep.x;
                kl.ePointInOctaveY = kl.endPointY = ep.y;
                kl.sPointInOctaveX = kl.startPointX = sp.x;
                kl.sPointInOctaveY = kl.startPointY = sp.y;
                kl.lineLength = (float) sqrt(pow(ep.y-sp.y,2)+pow(ep.x-sp.x,2));
                kl.class_id = lines[i].class_id;
                kl.octave = lines[i].octave;
                kl.size = (kl.endPointY-kl.startPointY)*(kl.endPointX-kl.startPointX);
                kl.response = kl.lineLength / 640;
                kl.pt = Point2f(((ep.x+sp.x)/2),((ep.y+sp.y)/2));
                LineIterator liter( image, sp, ep);
                kl.numOfPixels = liter.count;

                linetmp.push_back(kl);
                mergeflag[i] = true;
                mergeflag[j] = true;
                cflag = true;
#ifdef debugmesg
                cout<<"fit line"<<endl;
#endif

//                cout<<"d_des : "<< d_des <<endl;
//                cout<<"angi:  "<<angi<<endl;
//                cout<<"angj:  "<<angj<<endl;
//                cout<<"d_angle:  "<<d_angle<<endl;
//                cout<<"mid point d_dis:   "<<d_dis<<endl;
//                cout<<"line endpoint min distance:   "<<l_dis<<endl;

                break;
            }
        }
        for(int i=0;i<lines.size();i++){
            if(mergeflag[i]==true) continue;
            linetmp.push_back(lines[i]);
        }
#ifdef debugmesg
        cout<<"improve linetmp size:   "<<linetmp.size()<<endl;
#endif

        Ptr<BinaryDescriptor> lbdtmp = BinaryDescriptor::createBinaryDescriptor();//line_descriptor::BinaryDescriptor类实现了检测线和计算其二进制描述符的功能
        lbdtmp->compute(image, linetmp, ldesctmp);  //计算描述线特征的二进制描述子
        lines = linetmp;
        ldesc = ldesctmp;
    }
    keylines = lines;
}

int image_process(string img1path, string img2path, string outpath, string paramfilepath, bool saveLR = false)
{
    //load first image from file
    std::string imageName1 = img1path;
    std::string imageName2 = img2path;
    cout << "imgProcess: " << imageName1 << " + " << imageName2 << " = " << outpath << endl;
    cv::Mat leftImage;
    cv::Mat rightImage;
    cv::Mat binaryleftImage;
    cv::Mat binaryrightImage;
    cv::Mat knnleftImage;
    cv::Mat knnrightImage;
    cv::Mat lsd_leftImage;
    cv::Mat lsd_rightImage;
    cv::Mat testlsd_leftImage;
    cv::Mat testlsd_rightImage;

    leftImage = imread(imageName1, cv::IMREAD_GRAYSCALE);  // Read the file
    rightImage = imread(imageName2, cv::IMREAD_GRAYSCALE); // Read the file
    binaryleftImage = imread(imageName1, cv::IMREAD_GRAYSCALE);  // Read the file
    binaryrightImage = imread(imageName2, cv::IMREAD_GRAYSCALE); // Read the file
    knnleftImage = imread(imageName1, cv::IMREAD_GRAYSCALE);  // Read the file
    knnrightImage = imread(imageName2, cv::IMREAD_GRAYSCALE); // Read the file
    lsd_leftImage = imread(imageName1, cv::IMREAD_GRAYSCALE);  // Read the file
    lsd_rightImage = imread(imageName2, cv::IMREAD_GRAYSCALE); // Read the file
    testlsd_leftImage = imread(imageName1, cv::IMREAD_GRAYSCALE);  // Read the file
    testlsd_rightImage = imread(imageName2, cv::IMREAD_GRAYSCALE); // Read the file
    if (!leftImage.data || !rightImage.data)			   // Check for invalid input
    {
        cout << "Could not open or find the image" << std::endl;
        return -1;
    }

    unsigned int imageWidth = leftImage.cols; //图片的宽与高
    unsigned int imageHeight = leftImage.rows;

    srand((unsigned)time(0));
    int lowest = 100, highest = 255;
    int range = (highest - lowest) + 1;
    unsigned int r, g, b; //the color of lines
    cv::Point startPoint;
    cv::Point endPoint;
    cv::Point point;
    int linethickness = 3;

    //initial variables
    cv::Mat leftColorImage(leftImage.size(), CV_8UC3);
    cv::Mat rightColorImage(rightImage.size(), CV_8UC3);
    cvtColor(leftImage, leftColorImage, cv::COLOR_GRAY2RGB);
    cvtColor(rightImage, rightColorImage, cv::COLOR_GRAY2RGB);

    cv::Mat binaryleftColorImage(binaryleftImage.size(), CV_8UC3);
    cv::Mat binaryrightColorImage(binaryrightImage.size(), CV_8UC3);
    cvtColor(binaryleftImage, binaryleftColorImage, cv::COLOR_GRAY2RGB);
    cvtColor(binaryrightImage, binaryrightColorImage, cv::COLOR_GRAY2RGB);

    cv::Mat knnleftColorImage(knnleftImage.size(), CV_8UC3);
    cv::Mat knnrightColorImage(knnrightImage.size(), CV_8UC3);
    cvtColor(knnleftImage, knnleftColorImage, cv::COLOR_GRAY2RGB);
    cvtColor(knnrightImage, knnrightColorImage, cv::COLOR_GRAY2RGB);

    cv::Mat lsd_leftColorImage(lsd_leftImage.size(),CV_8UC3);
    cv::Mat lsd_rightColorImage(lsd_rightImage.size(),CV_8UC3);
    cvtColor(lsd_leftImage, lsd_leftColorImage, cv::COLOR_GRAY2RGB);
    cvtColor(lsd_rightImage, lsd_rightColorImage, cv::COLOR_GRAY2RGB);

    cv::Mat testlsd_leftColorImage(testlsd_leftImage.size(),CV_8UC3);
    cv::Mat testlsd_rightColorImage(testlsd_rightImage.size(),CV_8UC3);
    cvtColor(testlsd_leftImage, testlsd_leftColorImage, cv::COLOR_GRAY2RGB);
    cvtColor(testlsd_rightImage, testlsd_rightColorImage, cv::COLOR_GRAY2RGB);

    ///////////####################################################################
    ///////////####################################################################
    //extract lines, compute their descriptors and match lines
    //提取线段，计算descriptor并进行线段匹配
    LineDescriptor lineDesc;
    LineDescriptor binarylineDesc;

    PairwiseLineMatching lineMatch;
    PairwiseLineMatching binarylineMatch;

    ScaleLines linesInLeft;
    ScaleLines linesInRight;
    std::vector<unsigned int> matchResult;
    vector<KeyLine> keylinesInLeft;
    vector<KeyLine> keylinesInRight;
    vector<unsigned int> binarymatchResult;
    cv::Mat binary_left_ldesc,binary_right_ldesc;

    vector<KeyLine> knnkeylinesInLeft;
    vector<KeyLine> knnkeylinesInRight;
    vector<unsigned int> knnmatchResult;
    cv::Mat knn_left_ldesc,knn_right_ldesc;

    double dt;
    std::chrono::steady_clock::time_point t1,t2;

    t1 = std::chrono::steady_clock::now();
    lineDesc.GetLineDescriptor(leftImage, linesInLeft);
    t2 = std::chrono::steady_clock::now();
    dt = std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
    cout<<"EDLINE detect&compute lbd time: "<<dt<<endl;
    lineDesc.GetLineDescriptor(rightImage, linesInRight);
    cout<<"-------------------------"<<endl;
    cout<<endl;

    t1 = std::chrono::steady_clock::now();
    binarylineDesc.GetLineBinaryDescriptor(binaryleftImage, keylinesInLeft,binary_left_ldesc);
    t2 = std::chrono::steady_clock::now();
    dt = std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
    cout<<"BINARY EDLINE detect&compute lbd time: "<<dt<<endl;
    binarylineDesc.GetLineBinaryDescriptor(binaryrightImage, keylinesInRight,binary_right_ldesc);
    cout<<"-------------------------"<<endl;
    cout<<endl;

    cv::Mat lsd_leftLdesc,lsd_rightLdesc;
    vector<KeyLine> lsd_leftkeylines;
    vector<KeyLine> lsd_rightkeylines;
    float scale = 1.2;
    int numOctaves = 1;

    t1 = std::chrono::steady_clock::now();
    LSDExtractLineSegment(lsd_leftImage,lsd_leftkeylines,scale,numOctaves,lsd_leftLdesc);
    t2 = std::chrono::steady_clock::now();
    dt = std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
    cout<<"LSD detect&compute lbd time: "<<dt<<endl;
    LSDExtractLineSegment(lsd_rightImage,lsd_rightkeylines,scale,numOctaves,lsd_rightLdesc);
    cout<<"-------------------------"<<endl;
    cout<<endl;

    for (vector<KeyLine>::iterator it = lsd_leftkeylines.begin(); it != lsd_leftkeylines.end(); ++it) {
        r = lowest + int(rand() % range);
        g = lowest + int(rand() % range);
        b = lowest + int(rand() % range);
        startPoint = cv::Point(int(it->startPointX), int(it->startPointY));
        endPoint = cv::Point(int(it->endPointX), int(it->endPointY));
        cv::line(lsd_leftColorImage, startPoint, endPoint, cv::Scalar(r, g, b), linethickness, cv::LINE_AA, 0);
//        cout<<"line start point in octaveX"<<it->sPointInOctaveX<<endl;
//        cout<<"line end point in octaveX"<<it->ePointInOctaveX<<endl;
//        cout<<"line start point"<<it->startPointX<<endl;
//        cout<<"line end point"<<it->endPointX<<endl;

//        cout<<"line angle : "<<it->angle/3.14159*180<<endl;
//        cout<<"line atan2 : "<<atan2(it->endPointY-it->startPointY,it->endPointX-it->startPointX)/3.14159*180<<endl;

    }
    for (vector<KeyLine>::iterator it = lsd_rightkeylines.begin(); it != lsd_rightkeylines.end(); ++it) {
        r = lowest + int(rand() % range);
        g = lowest + int(rand() % range);
        b = lowest + int(rand() % range);
        startPoint = cv::Point(int(it->startPointX), int(it->startPointY));
        endPoint = cv::Point(int(it->endPointX), int(it->endPointY));
        cv::line(lsd_rightColorImage, startPoint, endPoint, cv::Scalar(r, g, b), linethickness, cv::LINE_AA, 0);
    }
    cv::imwrite(img1path+"LSDLINE1.png", lsd_leftColorImage);
    cv::imwrite(img2path+"LSDLINE2.png", lsd_rightColorImage);
    cout<<"LSD get left image line numbers : "<< lsd_leftkeylines.size() <<endl;
    cout<<"LSD get right image line numbers : "<< lsd_rightkeylines.size() <<endl;
    cout<<"-------------------------"<<endl;
    cout<<endl;

    cv::Mat testlsd_leftLdesc,testlsd_rightLdesc;
    vector<KeyLine> testlsd_leftkeylines;
    vector<KeyLine> testlsd_rightkeylines;

    t1 = std::chrono::steady_clock::now();
    LSDExtractLineSegmentImprove(testlsd_leftImage,testlsd_leftkeylines,scale,numOctaves,testlsd_leftLdesc);
    t2 = std::chrono::steady_clock::now();
    dt = std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
    cout<<"Improve LSD detect&compute lbd time: "<<dt<<endl;

    for (vector<KeyLine>::iterator it = testlsd_leftkeylines.begin(); it != testlsd_leftkeylines.end(); ++it) {
        r = lowest + int(rand() % range);
        g = lowest + int(rand() % range);
        b = lowest + int(rand() % range);
        startPoint = cv::Point(int(it->startPointX), int(it->startPointY));
        endPoint = cv::Point(int(it->endPointX), int(it->endPointY));
        cv::line(testlsd_leftColorImage, startPoint, endPoint, cv::Scalar(r, g, b), linethickness, cv::LINE_AA, 0);
    }
    cv::imwrite(img1path+"testLSDLINE1.png", testlsd_leftColorImage);
    cout<<"LSD get test left image line numbers : "<< testlsd_leftkeylines.size() <<endl;

    LSDExtractLineSegmentImprove(testlsd_rightImage,testlsd_rightkeylines,scale,numOctaves,testlsd_rightLdesc);
    for (vector<KeyLine>::iterator it = testlsd_rightkeylines.begin(); it != testlsd_rightkeylines.end(); ++it) {
        r = lowest + int(rand() % range);
        g = lowest + int(rand() % range);
        b = lowest + int(rand() % range);
        startPoint = cv::Point(int(it->startPointX), int(it->startPointY));
        endPoint = cv::Point(int(it->endPointX), int(it->endPointY));
        cv::line(testlsd_rightColorImage, startPoint, endPoint, cv::Scalar(r, g, b), linethickness, cv::LINE_AA, 0);
    }
    cv::imwrite(img2path+"testLSDLINE2.png", testlsd_rightColorImage);
    cout<<"LSD get test right image line numbers : "<< testlsd_rightkeylines.size() <<endl;
    //    knnkeylinesInLeft = keylinesInLeft;
//    knnkeylinesInRight = keylinesInRight;
//    knn_left_ldesc = binary_left_ldesc;
//    knn_right_ldesc = binary_right_ldesc;
    knnkeylinesInLeft = testlsd_leftkeylines;
    knnkeylinesInRight = testlsd_rightkeylines;
    knn_left_ldesc = testlsd_leftLdesc;
    knn_right_ldesc = testlsd_rightLdesc;
    cout<<"-------------------------"<<endl;
    cout<<endl;


    //TODO remove BIAS dependecies in PairwiseMatching
    t1 = std::chrono::steady_clock::now();
    lineMatch.LineMatching(linesInLeft, linesInRight, matchResult);
    t2 = std::chrono::steady_clock::now();
    dt = std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
    cout<<"normal EDLINE match time: "<<dt<<endl;

    t1 = std::chrono::steady_clock::now();
//    binarylineMatch.BinaryLineMatching(keylinesInLeft, keylinesInRight,binary_left_ldesc,binary_right_ldesc, binarymatchResult);
    binarylineMatch.BinaryLineMatching(testlsd_leftkeylines, testlsd_rightkeylines,testlsd_leftLdesc,testlsd_rightLdesc, binarymatchResult);
    t2 = std::chrono::steady_clock::now();
    dt = std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
    cout<<"BINARY EDLINE match time: "<<dt<<endl;

    t1 = std::chrono::steady_clock::now();
    SearchByDescriptor(knnkeylinesInLeft, knnkeylinesInRight, knn_left_ldesc, knn_right_ldesc, knnmatchResult);
    t2 = std::chrono::steady_clock::now();
    dt = std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
    cout<<"KNN EDLINE match time: "<<dt<<endl;
    cout<<"-------------------------"<<endl;
    cout<<endl;
    //save the param of matching
//    if(!paramfilepath.empty())
//    {
//        svScaleLines(linesInLeft,paramfilepath,"Llines");
//        svScaleLines(linesInRight,paramfilepath,"Rlines");
//        svMatchResult(matchResult,paramfilepath,"MatchRes");
//    }
    ///////////####################################################################
    ///////////####################################################################
    //draw  extracted lines into images


    for (unsigned int i = 0; i < linesInLeft.size(); i++)
    {
        r = lowest + int(rand() % range);
        g = lowest + int(rand() % range);
        b = lowest + int(rand() % range);
        startPoint = cv::Point(int(linesInLeft[i][0].startPointX), int(linesInLeft[i][0].startPointY));
        endPoint = cv::Point(int(linesInLeft[i][0].endPointX), int(linesInLeft[i][0].endPointY));
        cv::line(leftColorImage, startPoint, endPoint, cv::Scalar(r, g, b), linethickness, cv::LINE_AA, 0);
    }
    for (unsigned int i = 0; i < linesInRight.size(); i++)
    {
        r = lowest + int(rand() % range);
        g = lowest + int(rand() % range);
        b = lowest + int(rand() % range);
        startPoint = cv::Point(int(linesInRight[i][0].startPointX), int(linesInRight[i][0].startPointY));
        endPoint = cv::Point(int(linesInRight[i][0].endPointX), int(linesInRight[i][0].endPointY));
        cv::line(rightColorImage, startPoint, endPoint, cv::Scalar(r, g, b), linethickness, cv::LINE_AA, 0);
    }
//    if (saveLR)
//    {
//        imwrite("LinesInImage1.png", leftColorImage);
//        imwrite("LinesInImage2.png", rightColorImage);
//    }
    cv::imwrite(img1path+"EDLINE1.png", leftColorImage);
    cv::imwrite(img2path+"EDLINE2.png", rightColorImage);

    cout<<"EDLINE get left image line numbers : "<< linesInLeft.size() <<endl;
    cout<<"EDLINE get right image line numbers : "<< linesInRight.size() <<endl;
    cout<<"-------------------------"<<endl;
    cout<<endl;

    for (unsigned int i = 0; i < keylinesInLeft.size(); i++)
    {
        r = lowest + int(rand() % range);
        g = lowest + int(rand() % range);
        b = lowest + int(rand() % range);
        startPoint = cv::Point(int(keylinesInLeft[i].startPointX), int(keylinesInLeft[i].startPointY));
        endPoint = cv::Point(int(keylinesInLeft[i].endPointX), int(keylinesInLeft[i].endPointY));
        cv::line(binaryleftColorImage, startPoint, endPoint, cv::Scalar(r, g, b), linethickness, cv::LINE_AA, 0);
        cv::line(knnleftColorImage, startPoint, endPoint, cv::Scalar(r, g, b), linethickness, cv::LINE_AA, 0);
    }
    for (unsigned int i = 0; i < keylinesInRight.size(); i++)
    {
        r = lowest + int(rand() % range);
        g = lowest + int(rand() % range);
        b = lowest + int(rand() % range);
        startPoint = cv::Point(int(keylinesInRight[i].startPointX), int(keylinesInRight[i].startPointY));
        endPoint = cv::Point(int(keylinesInRight[i].endPointX), int(keylinesInRight[i].endPointY));
        cv::line(binaryrightColorImage, startPoint, endPoint, cv::Scalar(r, g, b), linethickness, cv::LINE_AA, 0);
        cv::line(knnrightColorImage, startPoint, endPoint, cv::Scalar(r, g, b), linethickness, cv::LINE_AA, 0);

    }
//    if (saveLR)
//    {
//        imwrite("binaryLinesInImage1.png", leftColorImage);
//        imwrite("binaryLinesInImage2.png", rightColorImage);
//    }
    cv::imwrite(img1path+"binaryEDLINE1.png", leftColorImage);
    cv::imwrite(img2path+"binaryEDLINE2.png", rightColorImage);

    cout<<"BIANRY EDLINE get left image line numbers : "<< linesInLeft.size() <<endl;
    cout<<"BINARY EDLINE get right image line numbers : "<< linesInRight.size() <<endl;
    cout<<"-------------------------"<<endl;
    cout<<endl;

    //TODO enable after BIAS dependecies in PairwiseMatching have been removed
    //  ///////////####################################################################
    //  ///////////####################################################################
    //  //store the matching results of the first and second images into a single image
    double ww1, ww2;
    int lineIDLeft;
    int lineIDRight;
    int lowest1 = 0, highest1 = 255;
    int range1 = (highest1 - lowest1) + 1;
    std::vector<unsigned int> r1(matchResult.size() / 2), g1(matchResult.size() / 2), b1(matchResult.size() / 2); //the color of lines
    std::vector<unsigned int> r2(binarymatchResult.size() / 2), g2(binarymatchResult.size() / 2), b2(binarymatchResult.size() / 2); //the color of lines
    std::vector<unsigned int> r3(knnmatchResult.size()/2),g3(knnmatchResult.size()/2),b3(knnmatchResult.size()/2);
    for (unsigned int pair = 0; pair < matchResult.size() / 2; pair++)
    {
        r1[pair] = lowest1 + int(rand() % range1);
        g1[pair] = lowest1 + int(rand() % range1);
        b1[pair] = 255 - r1[pair];
        ww1 = 0.2 * (rand() % 5);
        ww2 = 1 - ww1;
        char buf[10];
        sprintf(buf, "%d ", pair);
        lineIDLeft = matchResult[2 * pair];
        lineIDRight = matchResult[2 * pair + 1];
        startPoint = cv::Point(int(linesInLeft[lineIDLeft][0].startPointX), int(linesInLeft[lineIDLeft][0].startPointY));
        endPoint = cv::Point(int(linesInLeft[lineIDLeft][0].endPointX), int(linesInLeft[lineIDLeft][0].endPointY));
        cv::line(leftColorImage, startPoint, endPoint, CV_RGB(r1[pair], g1[pair], b1[pair]), 4, cv::LINE_AA, 0);
        startPoint = cvPoint(int(linesInRight[lineIDRight][0].startPointX), int(linesInRight[lineIDRight][0].startPointY));
        endPoint = cvPoint(int(linesInRight[lineIDRight][0].endPointX), int(linesInRight[lineIDRight][0].endPointY));
        cv::line(rightColorImage, startPoint, endPoint, CV_RGB(r1[pair], g1[pair], b1[pair]), 4, cv::LINE_AA, 0);
    }

    cv::Mat cvResultColorImage1 = cv::Mat(cv::Size(imageWidth * 2, imageHeight), leftColorImage.type(), 3);
    cv::Mat cvResultColorImage2 = cv::Mat(cv::Size(imageWidth * 2, imageHeight), leftColorImage.type(), 3);
    cv::Mat cvResultColorImage = cv::Mat(cv::Size(imageWidth * 2, imageHeight), leftColorImage.type(), 3);
    cv::Mat roi = cvResultColorImage1(cv::Rect(0, 0, imageWidth, imageHeight));
    cv::resize(leftColorImage, roi, roi.size(), 0, 0, 0);

    cv::Mat roi2 = cvResultColorImage1(cv::Rect(imageWidth, 0, imageWidth, imageHeight));
    cv::resize(rightColorImage, roi2, roi2.size(), 0, 0, 0);
    cvResultColorImage1.copyTo(cvResultColorImage2);

    for (unsigned int pair = 0; pair < matchResult.size() / 2; pair++)
    {
        lineIDLeft = matchResult[2 * pair];
        lineIDRight = matchResult[2 * pair + 1];
//        startPoint = cv::Point(int((linesInLeft[lineIDLeft][0].startPointX+linesInLeft[lineIDLeft][0].endPointX)/2),
//                               int((linesInLeft[lineIDLeft][0].startPointY+linesInLeft[lineIDLeft][0].endPointY)/2));
//        endPoint = cv::Point(int((linesInRight[lineIDRight][0].startPointX+linesInRight[lineIDRight][0].endPointX)/2 + imageWidth),
//                             int((linesInRight[lineIDRight][0].startPointY+linesInRight[lineIDRight][0].endPointY)/2));
        startPoint = cv::Point(int((linesInLeft[lineIDLeft][0].startPointX)),
                               int((linesInLeft[lineIDLeft][0].startPointY)));
        endPoint = cv::Point(int((linesInRight[lineIDRight][0].startPointX) + imageWidth),
                             int((linesInRight[lineIDRight][0].startPointY)));
        cv::line(cvResultColorImage2, startPoint, endPoint, CV_RGB(r1[pair], g1[pair], b1[pair]), 2, cv::LINE_AA, 0);
    }
    cv::addWeighted(cvResultColorImage1, 0.5, cvResultColorImage2, 0.5, 0.0, cvResultColorImage, -1);

    cv::imwrite(outpath+".png", cvResultColorImage);
    cout << "number of total matches = " << matchResult.size() / 2 << endl;

///////////////////////////////////////////binary match///////////////////////////////////////////////////
    for (unsigned int pair = 0; pair < binarymatchResult.size() / 2; pair++)
    {
        r2[pair] = lowest1 + int(rand() % range1);
        g2[pair] = lowest1 + int(rand() % range1);
        b2[pair] = 255 - r2[pair];
        ww1 = 0.2 * (rand() % 5);
        ww2 = 1 - ww1;
        char buf[10];
        sprintf(buf, "%d ", pair);
        lineIDLeft = binarymatchResult[2 * pair];
        lineIDRight = binarymatchResult[2 * pair + 1];
//        startPoint = cv::Point(int(keylinesInLeft[lineIDLeft].startPointX), int(keylinesInLeft[lineIDLeft].startPointY));
//        endPoint = cv::Point(int(keylinesInLeft[lineIDLeft].endPointX), int(keylinesInLeft[lineIDLeft].endPointY));
//        cv::line(binaryleftColorImage, startPoint, endPoint, CV_RGB(r2[pair], g2[pair], b2[pair]), 4, cv::LINE_AA, 0);
//        startPoint = cvPoint(int(keylinesInRight[lineIDRight].startPointX), int(keylinesInRight[lineIDRight].startPointY));
//        endPoint = cvPoint(int(keylinesInRight[lineIDRight].endPointX), int(keylinesInRight[lineIDRight].endPointY));
//        cv::line(binaryrightColorImage, startPoint, endPoint, CV_RGB(r2[pair], g2[pair], b2[pair]), 4, cv::LINE_AA, 0);
//        //binarylineMatch.BinaryLineMatching(testlsd_leftkeylines, testlsd_rightkeylines,testlsd_leftLdesc,testlsd_rightLdesc, binarymatchResult);
        startPoint = cv::Point(int(testlsd_leftkeylines[lineIDLeft].startPointX), int(testlsd_leftkeylines[lineIDLeft].startPointY));
        endPoint = cv::Point(int(testlsd_leftkeylines[lineIDLeft].endPointX), int(testlsd_leftkeylines[lineIDLeft].endPointY));
        cv::line(binaryleftColorImage, startPoint, endPoint, CV_RGB(r2[pair], g2[pair], b2[pair]), 4, cv::LINE_AA, 0);
        startPoint = cvPoint(int(testlsd_rightkeylines[lineIDRight].startPointX), int(testlsd_rightkeylines[lineIDRight].startPointY));
        endPoint = cvPoint(int(testlsd_rightkeylines[lineIDRight].endPointX), int(testlsd_rightkeylines[lineIDRight].endPointY));
        cv::line(binaryrightColorImage, startPoint, endPoint, CV_RGB(r2[pair], g2[pair], b2[pair]), 4, cv::LINE_AA, 0);

    }

    cv::Mat binarycvResultColorImage1 = cv::Mat(cv::Size(imageWidth * 2, imageHeight), binaryleftColorImage.type(), 3);
    cv::Mat binarycvResultColorImage2 = cv::Mat(cv::Size(imageWidth * 2, imageHeight), binaryleftColorImage.type(), 3);
    cv::Mat binarycvResultColorImage = cv::Mat(cv::Size(imageWidth * 2, imageHeight), binaryleftColorImage.type(), 3);
    cv::Mat binaryroi = binarycvResultColorImage1(cv::Rect(0, 0, imageWidth, imageHeight));
    cv::resize(binaryleftColorImage, binaryroi, binaryroi.size(), 0, 0, 0);

    cv::Mat binaryroi2 = binarycvResultColorImage1(cv::Rect(imageWidth, 0, imageWidth, imageHeight));
    cv::resize(binaryrightColorImage, binaryroi2, binaryroi2.size(), 0, 0, 0);
    binarycvResultColorImage1.copyTo(binarycvResultColorImage2);

    for (unsigned int pair = 0; pair < binarymatchResult.size() / 2; pair++)
    {
        lineIDLeft = binarymatchResult[2 * pair];
        lineIDRight = binarymatchResult[2 * pair + 1];
//        startPoint = cv::Point(int((keylinesInLeft[lineIDLeft].startPointX+keylinesInLeft[lineIDLeft].endPointX)/2),
//                               int((keylinesInLeft[lineIDLeft].startPointY+keylinesInLeft[lineIDLeft].endPointY)/2));
//        endPoint = cv::Point(int((keylinesInRight[lineIDRight].startPointX+keylinesInRight[lineIDRight].endPointX)/2 + imageWidth),
//                             int((keylinesInRight[lineIDRight].startPointY+keylinesInRight[lineIDRight].endPointY)/2));
//        startPoint = cv::Point(int((keylinesInLeft[lineIDLeft].startPointX)),
//                               int(keylinesInLeft[lineIDLeft].startPointY));
//        endPoint = cv::Point(int((keylinesInRight[lineIDRight].startPointX) + imageWidth),
//                             int((keylinesInRight[lineIDRight].startPointY)));
        startPoint = cv::Point(int((testlsd_leftkeylines[lineIDLeft].startPointX)),
                               int(testlsd_leftkeylines[lineIDLeft].startPointY));
        endPoint = cv::Point(int((testlsd_rightkeylines[lineIDRight].startPointX) + imageWidth),
                             int((testlsd_rightkeylines[lineIDRight].startPointY)));

        cv::line(binarycvResultColorImage2, startPoint, endPoint, CV_RGB(r2[pair], g2[pair], b2[pair]), 2, cv::LINE_AA, 0);
    }
    cv::addWeighted(binarycvResultColorImage1, 0.5, binarycvResultColorImage2, 0.5, 0.0, binarycvResultColorImage, -1);

    cv::imwrite(outpath+"binary.png", binarycvResultColorImage);
    cout << "number of total binary matches = " << binarymatchResult.size() / 2 << endl;

//////////////////////////////////////knn match/////////////////////////////////////////////////
    for (unsigned int pair = 0; pair < knnmatchResult.size() / 2; pair++)
    {
        r3[pair] = lowest1 + int(rand() % range1);
        g3[pair] = lowest1 + int(rand() % range1);
        b3[pair] = 255 - r3[pair];
        ww1 = 0.2 * (rand() % 5);
        ww2 = 1 - ww1;
        char buf[10];
        sprintf(buf, "%d ", pair);
        lineIDLeft = knnmatchResult[2 * pair];
        lineIDRight = knnmatchResult[2 * pair + 1];
        startPoint = cv::Point(int(knnkeylinesInLeft[lineIDLeft].startPointX), int(knnkeylinesInLeft[lineIDLeft].startPointY));
        endPoint = cv::Point(int(knnkeylinesInLeft[lineIDLeft].endPointX), int(knnkeylinesInLeft[lineIDLeft].endPointY));
        cv::line(knnleftColorImage, startPoint, endPoint, CV_RGB(r3[pair], g3[pair], b3[pair]), 4, cv::LINE_AA, 0);
        startPoint = cvPoint(int(knnkeylinesInRight[lineIDRight].startPointX), int(knnkeylinesInRight[lineIDRight].startPointY));
        endPoint = cvPoint(int(knnkeylinesInRight[lineIDRight].endPointX), int(knnkeylinesInRight[lineIDRight].endPointY));
        cv::line(knnrightColorImage, startPoint, endPoint, CV_RGB(r3[pair], g3[pair], b3[pair]), 4, cv::LINE_AA, 0);
    }

    cv::Mat knncvResultColorImage1 = cv::Mat(cv::Size(imageWidth * 2, imageHeight), knnleftColorImage.type(), 3);
    cv::Mat knncvResultColorImage2 = cv::Mat(cv::Size(imageWidth * 2, imageHeight), knnleftColorImage.type(), 3);
    cv::Mat knncvResultColorImage = cv::Mat(cv::Size(imageWidth * 2, imageHeight), knnleftColorImage.type(), 3);
    cv::Mat knnroi = knncvResultColorImage1(cv::Rect(0, 0, imageWidth, imageHeight));
    cv::resize(knnleftColorImage, knnroi, knnroi.size(), 0, 0, 0);

    cv::Mat knnroi2 = knncvResultColorImage1(cv::Rect(imageWidth, 0, imageWidth, imageHeight));
    cv::resize(knnrightColorImage, knnroi2, knnroi2.size(), 0, 0, 0);
    knncvResultColorImage1.copyTo(knncvResultColorImage2);

    for (unsigned int pair = 0; pair < knnmatchResult.size() / 2; pair++)
    {
        lineIDLeft = knnmatchResult[2 * pair];
        lineIDRight = knnmatchResult[2 * pair + 1];
//        startPoint = cv::Point(int((keylinesInLeft[lineIDLeft].startPointX+keylinesInLeft[lineIDLeft].endPointX)/2),
//                               int((keylinesInLeft[lineIDLeft].startPointY+keylinesInLeft[lineIDLeft].endPointY)/2));
//        endPoint = cv::Point(int((keylinesInRight[lineIDRight].startPointX+keylinesInRight[lineIDRight].endPointX)/2 + imageWidth),
//                             int((keylinesInRight[lineIDRight].startPointY+keylinesInRight[lineIDRight].endPointY)/2));
        startPoint = cv::Point(int((knnkeylinesInLeft[lineIDLeft].startPointX)),
                               int(knnkeylinesInLeft[lineIDLeft].startPointY));
        endPoint = cv::Point(int((knnkeylinesInRight[lineIDRight].startPointX) + imageWidth),
                             int((knnkeylinesInRight[lineIDRight].startPointY)));

        cv::line(knncvResultColorImage2, startPoint, endPoint, CV_RGB(r3[pair], g3[pair], b3[pair]), 2, cv::LINE_AA, 0);
    }
    cv::addWeighted(knncvResultColorImage1, 0.5, knncvResultColorImage2, 0.5, 0.0, knncvResultColorImage, -1);

    cv::imwrite(outpath+"knn.png", knncvResultColorImage);
    cout << "number of total knn matches = " << knnmatchResult.size() / 2 << endl;



    cvResultColorImage.release();
    cvResultColorImage1.release();
    cvResultColorImage2.release();
    leftImage.release();
    rightImage.release();
    leftColorImage.release();
    rightColorImage.release();
    roi.release();
    roi2.release();

    binarycvResultColorImage.release();
    binarycvResultColorImage1.release();
    binarycvResultColorImage2.release();
    binaryleftImage.release();
    binaryrightImage.release();
    binaryleftColorImage.release();
    binaryrightColorImage.release();
    binaryroi.release();
    binaryroi2.release();

    matchResult.clear();
    linesInLeft.clear();
    linesInRight.clear();

    binarymatchResult.clear();
    keylinesInLeft.clear();
    keylinesInRight.clear();
}
